/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Timer;

/**
 *
 * @author Team 1533
 */
public class CustomUltrasonics {

	Timer timer;
	private long counterValue = 0;
	private double speedOfSound_inchesPerMs = 13.772441;
	private double usonicTime[] = {0, 0};
	private boolean usonicIsLow[] = new boolean[2];
	DigitalInput[] usonicIn = new DigitalInput[2];
	DigitalOutput[] usonicSyncOut = new DigitalOutput[2];

	public CustomUltrasonics(int u1out, int u1in, int u2out, int u2in) {
		usonicIn[0] = new DigitalInput(u1in);
		usonicIn[1] = new DigitalInput(u2in);
		usonicSyncOut[0] = new DigitalOutput(u1out);
		usonicSyncOut[1] = new DigitalOutput(u2out);
		timer = new Timer();
	}

	public void startPinging() {
		usonicSyncOut[0].set(true);
		usonicSyncOut[1].set(true);

	}

	public void stopPinging() {
		usonicSyncOut[0].set(false);
		usonicSyncOut[1].set(false);
		timer.start();
		new UltrasonicWatchThread();
	}

	public double[] getTimesOfFlight() {
		return usonicTime;
	}

	public double[] getDistances() {
		double[] d = {calculateDistance(usonicTime[0]), calculateDistance(usonicTime[1])};
		return d;
	}

	public double calculateDistance(double ustime) {
		ustime /= 1000;
		return speedOfSound_inchesPerMs * ustime;
	}

	public class UltrasonicWatchThread extends Thread {

		public void run() {
			usonicTime[0] = usonicTime[1] = 0;
			do {
				for (int i = 0; i < usonicIn.length; i++) {
					if (!usonicIn[i].get() && !usonicIsLow[i]) {		//if the input is low
						usonicTime[i] = timer.get();				//store the time for the ultrasonic
						usonicIsLow[i] = false;
					}
				}
			}
			while ((!usonicIsLow[0] || !usonicIsLow[1]) && timer.get() < 50000);
		}
	}
}
